﻿
// 2020/1/16: 首个记录
// 2020/3/10: 增加红灯/绿灯闪烁状态
// 2020/5/15: 移除编译warning
// 2020/7/20: 更新样本标题
// 2020/11/2: 增加停车位是否被占用字段
// 2020/11/12: 支持Spadas7
// 2022/11/2: 支持Spadas8.4
// 2023/9/12: StopSign改为TrafficSign，并增加TrafficSignType
// 2023/10/11: 补充部分英文注释
// 2024/5/20: 新增TrafficLightData.stopPosX/Y
// 2024/7/3: 基于新样本框架。枚举改为使用enum class
// 2025/4/7: 实现插值和fromSample
// 2025/8/4: 不使用cat宏
// 2025/8/4: 改为继承SessionSample。改回单头文件

#ifndef TRAFFIC_SAMPLE_V2_H
#define TRAFFIC_SAMPLE_V2_H

#include "spadas.h"

namespace traffic_sample_v2
{
	using namespace spadas;

	// Elements
	enum class TrafficElementType
	{
		Invalid = 0,
		SpeedLimit = 1, // 限速标志
		TrafficSign = 2, // 交通标志
		TrafficLight = 3, // 交通信号灯
		StopLine = 4, // 停止线
		SpeedBump = 5, // 减速带
		ArrowMark = 6, // 路面箭头
		ProhibitionZone = 7, // 禁停区
		ParkingSlot = 8, // 停车位
		ZebraCrossing = 9, // 斑马线
	};

	enum class TrafficSignType
	{
		Stop = 0, // 停止标志

		NoEntry = 1, // 禁止驶入
		NoDriving = 2, // 禁止通行
		NoParking = 3, // 禁止停车
		NoLongTimeParking = 4, // 禁止长时间停车
		NoVehicle = 5, // 禁止机动车驶入
		NoTruck = 6, // 禁止货车驶入
		NoBus = 7, // 禁止客车驶入
		NoPedestrian = 8, // 禁止行人进入
		NoLeftTurn = 9, // 禁止左转
		NoRightTurn = 10, // 禁止右转
		NoAnyTurn = 11, // 禁止转弯
		NoForward = 12, // 禁止直行
		NoForwardOrLeft = 13, // 禁止直行或左转
		NoForwardOrRight = 14, // 禁止直行或右转
		NoUTurn = 15, // 禁止调头
		NoHorn = 16, // 禁止鸣笛
		NoOvertake = 17, // 禁止超车
		NoOvertakeEnder = 18, // 禁止超车区间结束
		NoDangerousGoods = 19, // 禁止危险品车辆驶入

		LowSpeedLimit = 50, // 低速限速
		SpeedLimitEnder = 51, // 限速区间结束
		WeightLimitWheel = 52, // 轴重限重
		WeightLimitTotal = 53, // 全车限重
		HeightLimit = 54, // 高度限制
		WidthLimit = 55, // 宽度限制
		Yield = 56, // 让行标志
		StopAndCheck = 57, // 停车检查

		GeneralSign = 100, // 指示类标志
		GeneralNoSpecialVehicle = 101, // 禁止特种车辆驶入
		GeneralConstruction = 102, // 施工标志
		GeneralWarning = 103, // 警告牌
		GeneralGreenBackground = 104, // 绿底指示牌
		GeneralBlueBackground = 105, // 蓝底指示牌
		GeneralRedBackground = 106, // 红底指示牌
		GeneralWhiteBackground = 107, // 白底指示牌
		GeneralYellowRoadID = 108, // 黄底道路ID牌
		GeneralRedRoadID = 109, // 红底道路ID牌
	};

	struct OptBool
	{
		Bool valid;
		Bool value;
	};

	struct OptDouble
	{
		Bool valid;
		Double value;
	};

	struct ArrowCombination
	{
		Bool valid; // FALSE while direction is not specified / 是否有效，若不分方向(如实心圆交通灯信号)则为FALSE
		Bool goStraight; // 直行
		Bool turnLeft; // 左转
		Bool turnRight; // 右转
		Bool uTurn; // 调头
	};

	struct SpeedLimitData
	{
		UInt limitedSpeed; // [KPH] 限速
		OptDouble posX; // [m] 纵向位置
		OptDouble posY; // [m] 横向位置
		OptDouble posZ; // [m] 垂直位置
	};

	struct TrafficSignData
	{
		TrafficSignType signType; // 交通标志类别
		OptDouble posX; // [m] 纵向位置
		OptDouble posY; // [m] 横向位置
		OptDouble posZ; // [m] 垂直位置
	};

	enum class TrafficLightState
	{
		Invalid = 0,
		Stop = 1, // 红灯
		Continue = 2, // 黄灯
		Caution = 3, // 黄灯闪烁
		Allow = 4, // 绿灯
		AllowFlash = 5, // 绿灯闪烁
		StopFlash = 6, // 红灯闪烁
	};

	struct TrafficLightData
	{
		TrafficLightState state; // 信号灯状态
		ArrowCombination arrows; // 信号灯的箭头组合
		OptDouble posX; // [m] 纵向位置
		OptDouble posY; // [m] 横向位置
		OptDouble posZ; // [m] 垂直位置
		OptDouble stopPosX; // [m] 纵向停止位置
		OptDouble stopPosY; // [m] 横向停止位置
	};

	struct StopLineData
	{
		Double distance; // [m] 停止线与x轴相交位置
		OptDouble heading; // [°] 停止线法线朝向，逆时针为正方向
	};

	struct SpeedBumpData
	{
		Double distance; // [m] 减速带与x轴相交位置
		OptDouble heading; // [°] 减速带法线朝向，逆时针为正方向
	};

	struct ArrowMarkData
	{
		ArrowCombination arrows; // 路面标志的箭头组合
		OptDouble posX; // [m] 纵向位置
		OptDouble posY; // [m] 横向位置
	};

	struct ProhibitionZoneData
	{
		Double centerX; // [m] 禁停区中央的纵向位置
		Double centerY; // [m] 禁停区中央的横向位置
		Double width; // [m] 禁停区的宽度
		Double length; // [m] 禁停区的长度
		OptDouble heading; // [°] 禁停区的朝向(沿长度方向)，逆时针为正方向
	};

	enum class ParkingSlotOccupancy
	{
		Unknown = 0, // 未知
		NotOccupied = 1, // 未被占用
		Occupied = 2, // 被占用
	};

	struct ParkingSlotData
	{
		Double centerX; // [m] 停车位中央的纵向位置
		Double centerY; // [m] 停车位中央的横向位置
		Double width; // [m] 停车位的宽度
		Double length; // [m] 停车位的长度
		OptDouble heading; // [°] 停车位的朝向(沿长度方向)，逆时针为正方向
		ParkingSlotOccupancy occupancy; // 停车位是否被占用
	};

	struct ZebraCrossingData
	{
		Double nearDistance; // [m] 斑马线近端与x轴相交位置
		OptDouble farDistance; // [m] 斑马线远端与x轴相交位置
		OptDouble heading; // [°] 斑马线的朝向，逆时针为正方向
	};

	// Sample
	union TrafficElementData
	{
		SpeedLimitData speedLimit;
		TrafficSignData trafficSign;
		TrafficLightData trafficLight;
		StopLineData stopLine;
		SpeedBumpData speedBump;
		ArrowMarkData arrowMark;
		ProhibitionZoneData prohibitionZone;
		ParkingSlotData parkingSlot;
		ZebraCrossingData zebraCrossing;
	};

	struct TrafficElement
	{
		TrafficElementType type; // 交通元素类别
		Int id; // 0 as invalid / 交通元素ID，0表示无效
		OptionalInt rawID; // 交通元素的原始ID
		TrafficElementData data; // 交通元素数据
	};

	struct TrafficSampleData
	{
		Array<TrafficElement> elements; // List of traffic elements / 交通元素列表

		TrafficSampleData()
		{}
	};

	class TrafficSample : public SessionSample
	{
	private:
		class TrafficSampleVars : public SessionSampleVars
		{
		public:
			SPADAS_VARS(TrafficSample, SessionSampleVars)
			TrafficSampleData data;

			SessionGeneralSample toGeneralSample()
			{
				SessionGeneralSample sample;
				sample.timestamp() = timestamp;

				Array<UInt> offsets(data.elements.size());
				Array<UInt> lengthes(data.elements.size());
				UInt totalDataSize = 0;
				for (UInt i = 0; i < data.elements.size(); i++)
				{
					UInt len = 0;
					switch (data.elements[i].type)
					{
					case TrafficElementType::SpeedLimit:
						len = 6;
						break;
					case TrafficElementType::TrafficSign:
						len = 5;
						break;
					case TrafficElementType::TrafficLight:
						len = 10;
						break;
					case TrafficElementType::StopLine:
						len = 4;
						break;
					case TrafficElementType::SpeedBump:
						len = 4;
						break;
					case TrafficElementType::ArrowMark:
						len = 8;
						break;
					case TrafficElementType::ProhibitionZone:
						len = 7;
						break;
					case TrafficElementType::ParkingSlot:
						len = 7;
						break;
					case TrafficElementType::ZebraCrossing:
						len = 5;
						break;
					default:
						break;
					}
					offsets[i] = totalDataSize;
					lengthes[i] = len;
					totalDataSize += len;
				}

				Array<GeneralElement> values(1 + data.elements.size() * 6 + totalDataSize);
				sample.values() = values;
				UInt base = 1 + data.elements.size() * 6;

				values[0] = (Double)data.elements.size();
				for (UInt i = 0; i < data.elements.size(); i++)
				{
					values[1 + 6 * i] = offsets[i];
					values[2 + 6 * i] = lengthes[i];
					values[3 + 6 * i] = (UInt)data.elements[i].type;
					values[4 + 6 * i] = data.elements[i].id;
					values[5 + 6 * i] = data.elements[i].rawID.valid ? (Double)data.elements[i].rawID.value : OptionalDouble();

					UInt eBase = base + offsets[i];
					switch (data.elements[i].type)
					{
					case TrafficElementType::SpeedLimit:
						values[eBase] = data.elements[i].data.speedLimit.limitedSpeed;
						values[eBase + 1] = convert(data.elements[i].data.speedLimit.posX);
						values[eBase + 2] = convert(data.elements[i].data.speedLimit.posY);
						values[eBase + 3] = convert(data.elements[i].data.speedLimit.posZ);
						break;
					case TrafficElementType::TrafficSign:
						values[eBase] = convert(data.elements[i].data.trafficSign.posX);
						values[eBase + 1] = convert(data.elements[i].data.trafficSign.posY);
						values[eBase + 2] = convert(data.elements[i].data.trafficSign.posZ);
						values[eBase + 3] = (UInt)data.elements[i].data.trafficSign.signType;
						break;
					case TrafficElementType::TrafficLight:
						values[eBase] = (UInt)data.elements[i].data.trafficLight.state;
						values[eBase + 1] = convert(data.elements[i].data.trafficLight.posX);
						values[eBase + 2] = convert(data.elements[i].data.trafficLight.posY);
						values[eBase + 3] = convert(data.elements[i].data.trafficLight.posZ);
						if (data.elements[i].data.trafficLight.arrows.valid)
						{
							values[eBase + 4] = data.elements[i].data.trafficLight.arrows.goStraight ? 1 : 0;
							values[eBase + 5] = data.elements[i].data.trafficLight.arrows.turnLeft ? 1 : 0;
							values[eBase + 6] = data.elements[i].data.trafficLight.arrows.turnRight ? 1 : 0;
							values[eBase + 7] = data.elements[i].data.trafficLight.arrows.uTurn ? 1 : 0;
						}
						values[eBase + 8] = convert(data.elements[i].data.trafficLight.stopPosX);
						values[eBase + 9] = convert(data.elements[i].data.trafficLight.stopPosY);
						break;
					case TrafficElementType::StopLine:
						values[eBase] = data.elements[i].data.stopLine.distance;
						values[eBase + 1] = convert(data.elements[i].data.stopLine.heading);
						break;
					case TrafficElementType::SpeedBump:
						values[eBase] = data.elements[i].data.speedBump.distance;
						values[eBase + 1] = convert(data.elements[i].data.speedBump.heading);
						break;
					case TrafficElementType::ArrowMark:
						values[eBase] = data.elements[i].data.arrowMark.arrows.goStraight ? 1 : 0;
						values[eBase + 1] = data.elements[i].data.arrowMark.arrows.turnLeft ? 1 : 0;
						values[eBase + 2] = data.elements[i].data.arrowMark.arrows.turnRight ? 1 : 0;
						values[eBase + 3] = data.elements[i].data.arrowMark.arrows.uTurn ? 1 : 0;
						values[eBase + 4] = convert(data.elements[i].data.arrowMark.posX);
						values[eBase + 5] = convert(data.elements[i].data.arrowMark.posY);
						break;
					case TrafficElementType::ProhibitionZone:
						values[eBase] = data.elements[i].data.prohibitionZone.centerX;
						values[eBase + 1] = data.elements[i].data.prohibitionZone.centerY;
						values[eBase + 2] = data.elements[i].data.prohibitionZone.width;
						values[eBase + 3] = data.elements[i].data.prohibitionZone.length;
						values[eBase + 4] = convert(data.elements[i].data.prohibitionZone.heading);
						break;
					case TrafficElementType::ParkingSlot:
						values[eBase] = data.elements[i].data.parkingSlot.centerX;
						values[eBase + 1] = data.elements[i].data.parkingSlot.centerY;
						values[eBase + 2] = data.elements[i].data.parkingSlot.width;
						values[eBase + 3] = data.elements[i].data.parkingSlot.length;
						values[eBase + 4] = convert(data.elements[i].data.parkingSlot.heading);
						values[eBase + 5] = (UInt)data.elements[i].data.parkingSlot.occupancy;
						break;
					case TrafficElementType::ZebraCrossing:
						values[eBase] = data.elements[i].data.zebraCrossing.nearDistance;
						values[eBase + 1] = convert(data.elements[i].data.zebraCrossing.farDistance);
						values[eBase + 2] = convert(data.elements[i].data.zebraCrossing.heading);
						break;
					default:
						break;
					}
				}

				sample.significantCount() = values.size();
				return sample;
			}

			Bool fromSample(String protocol, SessionSample sample) override
			{
				SessionGeneralSample generalSample;
				if (!sample.is<SessionGeneralSample>(generalSample)) return FALSE;

				timestamp = generalSample.timestamp();
				auto values = generalSample.values();

				if (protocol == "traffic-sample-v2")
				{
					if (values.size() < 1) return FALSE;
					if (!values[0].valid) return FALSE;

					Int count = (Int)values[0].value;
					if (values.size() < 1 + 6 * count) return FALSE;

					if (count > 0)
					{
						Int lastOffset = (Int)values[1 + 6 * (count - 1)].value;
						Int lastLength = (Int)values[2 + 6 * (count - 1)].value;
						Int totalDataSize = lastOffset + lastLength;
						if ((Int)values.size() != 1 + 6 * count + totalDataSize) return FALSE;
					}

					ArrayX<TrafficElement> elementsX;
					for (Int i = 0; i < count; i++)
					{
						if (!values[1 + 6 * i].valid || !values[2 + 6 * i].valid || 
							!values[3 + 6 * i].valid || !values[4 + 6 * i].valid) continue;

						auto type = (TrafficElementType)(Int)values[3 + 6 * i].value;
						if (type == TrafficElementType::Invalid) continue;

						Int base = 1 + 6 * count + (Int)values[1 + 6 * i].value;
						Int len = (Int)values[2 + 6 * i].value;
						Int id = (Int)values[4 + 6 * i].value;
						OptionalInt rawID;
						if (values[5 + 6 * i].valid) rawID = (Int)values[5 + 6 * i].value;

						TrafficElement newElem;
						newElem.type = type;
						newElem.id = id;
						newElem.rawID = rawID;

						Bool validElem = TRUE;
						switch (type)
						{
						case TrafficElementType::SpeedLimit:
							if (len != 6) 
							{
								validElem = FALSE;
								break;
							}
							newElem.data.speedLimit.limitedSpeed = (UInt)values[base].value;
							if (values[base + 1].valid)
							{
								newElem.data.speedLimit.posX.valid = TRUE;
								newElem.data.speedLimit.posX.value = values[base + 1].value;
							}
							if (values[base + 2].valid)
							{
								newElem.data.speedLimit.posY.valid = TRUE;
								newElem.data.speedLimit.posY.value = values[base + 2].value;
							}
							if (values[base + 3].valid)
							{
								newElem.data.speedLimit.posZ.valid = TRUE;
								newElem.data.speedLimit.posZ.value = values[base + 3].value;
							}
							break;
						case TrafficElementType::TrafficSign:
							if (len != 5)
							{
								validElem = FALSE;
								break;
							}
							newElem.data.trafficSign.signType = values[base + 3].valid ? 
								(TrafficSignType)(Int)values[base + 3].value : TrafficSignType::Stop;
							if (values[base].valid)
							{
								newElem.data.trafficSign.posX.valid = TRUE;
								newElem.data.trafficSign.posX.value = values[base].value;
							}
							if (values[base + 1].valid)
							{
								newElem.data.trafficSign.posY.valid = TRUE;
								newElem.data.trafficSign.posY.value = values[base + 1].value;
							}
							if (values[base + 2].valid)
							{
								newElem.data.trafficSign.posZ.valid = TRUE;
								newElem.data.trafficSign.posZ.value = values[base + 2].value;
							}
							break;
						case TrafficElementType::TrafficLight:
							if (len != 10)
							{
								validElem = FALSE;
								break;
							}
							newElem.data.trafficLight.state = (TrafficLightState)(Int)values[base].value;
							if (values[base + 1].valid)
							{
								newElem.data.trafficLight.posX.valid = TRUE;
								newElem.data.trafficLight.posX.value = values[base + 1].value;
							}
							if (values[base + 2].valid)
							{
								newElem.data.trafficLight.posY.valid = TRUE;
								newElem.data.trafficLight.posY.value = values[base + 2].value;
							}
							if (values[base + 3].valid)
							{
								newElem.data.trafficLight.posZ.valid = TRUE;
								newElem.data.trafficLight.posZ.value = values[base + 3].value;
							}
							if (values[base + 4].valid && values[base + 5].valid && 
								values[base + 6].valid && values[base + 7].valid)
							{
								newElem.data.trafficLight.arrows.valid = TRUE;
								newElem.data.trafficLight.arrows.goStraight = values[base + 4].value == 1;
								newElem.data.trafficLight.arrows.turnLeft = values[base + 5].value == 1;
								newElem.data.trafficLight.arrows.turnRight = values[base + 6].value == 1;
								newElem.data.trafficLight.arrows.uTurn = values[base + 7].value == 1;
							}
							if (values[base + 8].valid)
							{
								newElem.data.trafficLight.stopPosX.valid = TRUE;
								newElem.data.trafficLight.stopPosX.value = values[base + 8].value;
							}
							if (values[base + 9].valid)
							{
								newElem.data.trafficLight.stopPosY.valid = TRUE;
								newElem.data.trafficLight.stopPosY.value = values[base + 9].value;
							}
							break;
						case TrafficElementType::StopLine:
							if (len != 4)
							{
								validElem = FALSE;
								break;
							}
							newElem.data.stopLine.distance = values[base].value;
							if (values[base + 1].valid)
							{
								newElem.data.stopLine.heading.valid = TRUE;
								newElem.data.stopLine.heading.value = values[base + 1].value;
							}
							break;
						case TrafficElementType::SpeedBump:
							if (len != 4)
							{
								validElem = FALSE;
								break;
							}
							newElem.data.speedBump.distance = values[base].value;
							if (values[base + 1].valid)
							{
								newElem.data.speedBump.heading.valid = TRUE;
								newElem.data.speedBump.heading.value = values[base + 1].value;
							}
							break;
						case TrafficElementType::ArrowMark:
							if (len != 8)
							{
								validElem = FALSE;
								break;
							}
							newElem.data.arrowMark.arrows.valid = TRUE;
							newElem.data.arrowMark.arrows.goStraight = values[base].value == 1;
							newElem.data.arrowMark.arrows.turnLeft = values[base + 1].value == 1;
							newElem.data.arrowMark.arrows.turnRight = values[base + 2].value == 1;
							newElem.data.arrowMark.arrows.uTurn = values[base + 3].value == 1;
							if (values[base + 4].valid)
							{
								newElem.data.arrowMark.posX.valid = TRUE;
								newElem.data.arrowMark.posX.value = values[base + 4].value;
							}
							if (values[base + 5].valid)
							{
								newElem.data.arrowMark.posY.valid = TRUE;
								newElem.data.arrowMark.posY.value = values[base + 5].value;
							}
							break;
						case TrafficElementType::ProhibitionZone:
							if (len != 7)
							{
								validElem = FALSE;
								break;
							}
							newElem.data.prohibitionZone.centerX = values[base].value;
							newElem.data.prohibitionZone.centerY = values[base + 1].value;
							newElem.data.prohibitionZone.width = values[base + 2].value;
							newElem.data.prohibitionZone.length = values[base + 3].value;
							if (values[base + 4].valid)
							{
								newElem.data.prohibitionZone.heading.valid = TRUE;
								newElem.data.prohibitionZone.heading.value = values[base + 4].value;
							}
							break;
						case TrafficElementType::ParkingSlot:
							if (len != 7)
							{
								validElem = FALSE;
								break;
							}
							newElem.data.parkingSlot.centerX = values[base].value;
							newElem.data.parkingSlot.centerY = values[base + 1].value;
							newElem.data.parkingSlot.width = values[base + 2].value;
							newElem.data.parkingSlot.length = values[base + 3].value;
							if (values[base + 4].valid)
							{
								newElem.data.parkingSlot.heading.valid = TRUE;
								newElem.data.parkingSlot.heading.value = values[base + 4].value;
							}
							newElem.data.parkingSlot.occupancy = values[base + 5].valid ? 
								(ParkingSlotOccupancy)(Int)values[base + 5].value : ParkingSlotOccupancy::Unknown;
							break;
						case TrafficElementType::ZebraCrossing:
							if (len != 5)
							{
								validElem = FALSE;
								break;
							}
							newElem.data.zebraCrossing.nearDistance = values[base].value;
							if (values[base + 1].valid)
							{
								newElem.data.zebraCrossing.farDistance.valid = TRUE;
								newElem.data.zebraCrossing.farDistance.value = values[base + 1].value;
							}
							if (values[base + 2].valid)
							{
								newElem.data.zebraCrossing.heading.valid = TRUE;
								newElem.data.zebraCrossing.heading.value = values[base + 2].value;
							}
							break;
						default:
							validElem = FALSE;
							break;
						}

						if (validElem) elementsX.append(newElem);
					}
					data.elements = elementsX.toArray();
					return TRUE;
				}
				else if (protocol == "traffic-sample-v1")
				{
					if (values.size() < 1) return FALSE;
					if (!values[0].valid) return FALSE;

					Int count = (Int)values[0].value;
					if (values.size() < 1 + 3 * count) return FALSE;

					if (count > 0)
					{
						Int lastOffset = (Int)values[3 * count - 1].value;
						Int lastLength = (Int)values[3 * count].value;
						Int totalDataSize = lastOffset + lastLength;
						if ((Int)values.size() < 1 + 3 * count + totalDataSize) return FALSE;
					}

					ArrayX<TrafficElement> elementsX;
					for (Int i = 0; i < count; i++)
					{
						if (!values[1 + 3 * i].valid || !values[2 + 3 * i].valid) continue;

						auto type = (TrafficElementType)(Int)values[1 + 3 * i].value;
						if (type == TrafficElementType::Invalid) continue;

						Int base = 1 + 3 * count + (Int)values[2 + 3 * i].value;

						TrafficElement newElem;
						newElem.type = type;
						newElem.id = i + 1; // 自动分配ID

						Bool validElem = TRUE;
						switch (type)
						{
						case TrafficElementType::SpeedLimit:
							newElem.data.speedLimit.limitedSpeed = (UInt)values[base].value;
							if (values[base + 1].valid)
							{
								newElem.data.speedLimit.posX.valid = TRUE;
								newElem.data.speedLimit.posX.value = values[base + 1].value;
							}
							if (values[base + 2].valid)
							{
								newElem.data.speedLimit.posY.valid = TRUE;
								newElem.data.speedLimit.posY.value = values[base + 2].value;
							}
							break;
						case TrafficElementType::TrafficSign:
							newElem.data.trafficSign.signType = TrafficSignType::Stop;
							if (values[base].valid)
							{
								newElem.data.trafficSign.posX.valid = TRUE;
								newElem.data.trafficSign.posX.value = values[base].value;
							}
							if (values[base + 1].valid)
							{
								newElem.data.trafficSign.posY.valid = TRUE;
								newElem.data.trafficSign.posY.value = values[base + 1].value;
							}
							break;
						case TrafficElementType::TrafficLight:
							newElem.data.trafficLight.state = (TrafficLightState)(Int)values[base].value;
							if (values[base + 1].valid)
							{
								newElem.data.trafficLight.posX.valid = TRUE;
								newElem.data.trafficLight.posX.value = values[base + 1].value;
							}
							if (values[base + 2].valid)
							{
								newElem.data.trafficLight.posY.valid = TRUE;
								newElem.data.trafficLight.posY.value = values[base + 2].value;
							}
							if (values[base + 3].valid && values[base + 4].valid && 
								values[base + 5].valid && values[base + 6].valid)
							{
								newElem.data.trafficLight.arrows.valid = TRUE;
								newElem.data.trafficLight.arrows.goStraight = values[base + 3].value == 1;
								newElem.data.trafficLight.arrows.turnLeft = values[base + 4].value == 1;
								newElem.data.trafficLight.arrows.turnRight = values[base + 5].value == 1;
								newElem.data.trafficLight.arrows.uTurn = values[base + 6].value == 1;
							}
							break;
						case TrafficElementType::StopLine:
							newElem.data.stopLine.distance = values[base].value;
							if (values[base + 1].valid)
							{
								newElem.data.stopLine.heading.valid = TRUE;
								newElem.data.stopLine.heading.value = values[base + 1].value;
							}
							break;
						case TrafficElementType::SpeedBump:
							newElem.data.speedBump.distance = values[base].value;
							if (values[base + 1].valid)
							{
								newElem.data.speedBump.heading.valid = TRUE;
								newElem.data.speedBump.heading.value = values[base + 1].value;
							}
							break;
						case TrafficElementType::ArrowMark:
							newElem.data.arrowMark.arrows.valid = TRUE;
							newElem.data.arrowMark.arrows.goStraight = values[base].value == 1;
							newElem.data.arrowMark.arrows.turnLeft = values[base + 1].value == 1;
							newElem.data.arrowMark.arrows.turnRight = values[base + 2].value == 1;
							newElem.data.arrowMark.arrows.uTurn = values[base + 3].value == 1;
							if (values[base + 4].valid)
							{
								newElem.data.arrowMark.posX.valid = TRUE;
								newElem.data.arrowMark.posX.value = values[base + 4].value;
							}
							if (values[base + 5].valid)
							{
								newElem.data.arrowMark.posY.valid = TRUE;
								newElem.data.arrowMark.posY.value = values[base + 5].value;
							}
							break;
						case TrafficElementType::ProhibitionZone:
							newElem.data.prohibitionZone.centerX = values[base].value;
							newElem.data.prohibitionZone.centerY = values[base + 1].value;
							newElem.data.prohibitionZone.width = values[base + 2].value;
							newElem.data.prohibitionZone.length = values[base + 3].value;
							if (values[base + 4].valid)
							{
								newElem.data.prohibitionZone.heading.valid = TRUE;
								newElem.data.prohibitionZone.heading.value = values[base + 4].value;
							}
							break;
						case TrafficElementType::ParkingSlot:
							newElem.data.parkingSlot.centerX = values[base].value;
							newElem.data.parkingSlot.centerY = values[base + 1].value;
							newElem.data.parkingSlot.width = values[base + 2].value;
							newElem.data.parkingSlot.length = values[base + 3].value;
							if (values[base + 4].valid)
							{
								newElem.data.parkingSlot.heading.valid = TRUE;
								newElem.data.parkingSlot.heading.value = values[base + 4].value;
							}
							break;
						case TrafficElementType::ZebraCrossing:
							newElem.data.zebraCrossing.nearDistance = values[base].value;
							if (values[base + 1].valid)
							{
								newElem.data.zebraCrossing.farDistance.valid = TRUE;
								newElem.data.zebraCrossing.farDistance.value = values[base + 1].value;
							}
							if (values[base + 2].valid)
							{
								newElem.data.zebraCrossing.heading.valid = TRUE;
								newElem.data.zebraCrossing.heading.value = values[base + 2].value;
							}
							break;
						default:
							validElem = FALSE;
							break;
						}

						if (validElem) elementsX.append(newElem);
					}
					data.elements = elementsX.toArray();
					return TRUE;
				}

				return FALSE;
			}

			Bool supportInterpolation() override
			{
				return TRUE;
			}

			SessionSample interpolate(SessionSample& ss1, Double w1, SessionSample& ss2, Double w2, FullTimestamp timestamp) override
			{
				TrafficSample ts1, ts2;
				if (!ss1.is<TrafficSample>(ts1) || !ss2.is<TrafficSample>(ts2)) return SessionSample();

				TrafficSampleData& s1 = ts1.var()->data;
				TrafficSampleData& s2 = ts2.var()->data;
				
				// 检查元素是否有有效ID
				Bool noID = FALSE;
				for (auto elem = s1.elements.firstElem(); elem.valid(); ++elem)
				{
					if (elem->id <= 0)
					{
						noID = TRUE;
						break;
					}
				}
				
				for (auto elem = s2.elements.firstElem(); elem.valid(); ++elem)
				{
					if (elem->id <= 0)
					{
						noID = TRUE;
						break;
					}
				}
				
				// 如果没有有效ID，选择权重更大的那个
				if (noID)
				{
					return w1 > w2 ? ss1 : ss2;
				}
				
				// 创建输出对象
				TrafficSample output;
				output.timestamp() = timestamp;
				TrafficSampleData& so = output.var()->data;
				
				// 合并并插值交通元素
				ArrayX<TrafficElement> elementsX;
				
				for (auto s1Elem = s1.elements.firstElem(); s1Elem.valid(); ++s1Elem)
				{
					for (auto s2Elem = s2.elements.firstElem(); s2Elem.valid(); ++s2Elem)
					{
						if (s1Elem->id == s2Elem->id && s1Elem->type == s2Elem->type)
						{
							TrafficElement newElem;
							newElem.id = s1Elem->id;
							newElem.rawID = w1 > w2 ? s1Elem->rawID : s2Elem->rawID;
							newElem.type = s1Elem->type;
							
							Bool validType = TRUE;
							switch (s1Elem->type)
							{
							case TrafficElementType::SpeedLimit:
								newElem.data.speedLimit.limitedSpeed = w1 > w2 ? 
									s1Elem->data.speedLimit.limitedSpeed : s2Elem->data.speedLimit.limitedSpeed;
								
								if (s1Elem->data.speedLimit.posX.valid && s2Elem->data.speedLimit.posX.valid)
								{
									newElem.data.speedLimit.posX.valid = TRUE;
									newElem.data.speedLimit.posX.value = s1Elem->data.speedLimit.posX.value * w1 + 
										s2Elem->data.speedLimit.posX.value * w2;
								}
								else if (s1Elem->data.speedLimit.posX.valid)
								{
									newElem.data.speedLimit.posX = s1Elem->data.speedLimit.posX;
								}
								else if (s2Elem->data.speedLimit.posX.valid)
								{
									newElem.data.speedLimit.posX = s2Elem->data.speedLimit.posX;
								}
								
								if (s1Elem->data.speedLimit.posY.valid && s2Elem->data.speedLimit.posY.valid)
								{
									newElem.data.speedLimit.posY.valid = TRUE;
									newElem.data.speedLimit.posY.value = s1Elem->data.speedLimit.posY.value * w1 + 
										s2Elem->data.speedLimit.posY.value * w2;
								}
								else if (s1Elem->data.speedLimit.posY.valid)
								{
									newElem.data.speedLimit.posY = s1Elem->data.speedLimit.posY;
								}
								else if (s2Elem->data.speedLimit.posY.valid)
								{
									newElem.data.speedLimit.posY = s2Elem->data.speedLimit.posY;
								}
								
								if (s1Elem->data.speedLimit.posZ.valid && s2Elem->data.speedLimit.posZ.valid)
								{
									newElem.data.speedLimit.posZ.valid = TRUE;
									newElem.data.speedLimit.posZ.value = s1Elem->data.speedLimit.posZ.value * w1 + 
										s2Elem->data.speedLimit.posZ.value * w2;
								}
								else if (s1Elem->data.speedLimit.posZ.valid)
								{
									newElem.data.speedLimit.posZ = s1Elem->data.speedLimit.posZ;
								}
								else if (s2Elem->data.speedLimit.posZ.valid)
								{
									newElem.data.speedLimit.posZ = s2Elem->data.speedLimit.posZ;
								}
								break;
								
							case TrafficElementType::TrafficSign:
								newElem.data.trafficSign.signType = w1 > w2 ? 
									s1Elem->data.trafficSign.signType : s2Elem->data.trafficSign.signType;
									
								if (s1Elem->data.trafficSign.posX.valid && s2Elem->data.trafficSign.posX.valid)
								{
									newElem.data.trafficSign.posX.valid = TRUE;
									newElem.data.trafficSign.posX.value = s1Elem->data.trafficSign.posX.value * w1 + 
										s2Elem->data.trafficSign.posX.value * w2;
								}
								else if (s1Elem->data.trafficSign.posX.valid)
								{
									newElem.data.trafficSign.posX = s1Elem->data.trafficSign.posX;
								}
								else if (s2Elem->data.trafficSign.posX.valid)
								{
									newElem.data.trafficSign.posX = s2Elem->data.trafficSign.posX;
								}
								
								if (s1Elem->data.trafficSign.posY.valid && s2Elem->data.trafficSign.posY.valid)
								{
									newElem.data.trafficSign.posY.valid = TRUE;
									newElem.data.trafficSign.posY.value = s1Elem->data.trafficSign.posY.value * w1 + 
										s2Elem->data.trafficSign.posY.value * w2;
								}
								else if (s1Elem->data.trafficSign.posY.valid)
								{
									newElem.data.trafficSign.posY = s1Elem->data.trafficSign.posY;
								}
								else if (s2Elem->data.trafficSign.posY.valid)
								{
									newElem.data.trafficSign.posY = s2Elem->data.trafficSign.posY;
								}
								
								if (s1Elem->data.trafficSign.posZ.valid && s2Elem->data.trafficSign.posZ.valid)
								{
									newElem.data.trafficSign.posZ.valid = TRUE;
									newElem.data.trafficSign.posZ.value = s1Elem->data.trafficSign.posZ.value * w1 + 
										s2Elem->data.trafficSign.posZ.value * w2;
								}
								else if (s1Elem->data.trafficSign.posZ.valid)
								{
									newElem.data.trafficSign.posZ = s1Elem->data.trafficSign.posZ;
								}
								else if (s2Elem->data.trafficSign.posZ.valid)
								{
									newElem.data.trafficSign.posZ = s2Elem->data.trafficSign.posZ;
								}
								break;
								
							case TrafficElementType::TrafficLight:
								newElem.data.trafficLight.state = w1 > w2 ? 
									s1Elem->data.trafficLight.state : s2Elem->data.trafficLight.state;
								newElem.data.trafficLight.arrows = w1 > w2 ? 
									s1Elem->data.trafficLight.arrows : s2Elem->data.trafficLight.arrows;
									
								if (s1Elem->data.trafficLight.posX.valid && s2Elem->data.trafficLight.posX.valid)
								{
									newElem.data.trafficLight.posX.valid = TRUE;
									newElem.data.trafficLight.posX.value = s1Elem->data.trafficLight.posX.value * w1 + 
										s2Elem->data.trafficLight.posX.value * w2;
								}
								else if (s1Elem->data.trafficLight.posX.valid)
								{
									newElem.data.trafficLight.posX = s1Elem->data.trafficLight.posX;
								}
								else if (s2Elem->data.trafficLight.posX.valid)
								{
									newElem.data.trafficLight.posX = s2Elem->data.trafficLight.posX;
								}
								
								if (s1Elem->data.trafficLight.posY.valid && s2Elem->data.trafficLight.posY.valid)
								{
									newElem.data.trafficLight.posY.valid = TRUE;
									newElem.data.trafficLight.posY.value = s1Elem->data.trafficLight.posY.value * w1 + 
										s2Elem->data.trafficLight.posY.value * w2;
								}
								else if (s1Elem->data.trafficLight.posY.valid)
								{
									newElem.data.trafficLight.posY = s1Elem->data.trafficLight.posY;
								}
								else if (s2Elem->data.trafficLight.posY.valid)
								{
									newElem.data.trafficLight.posY = s2Elem->data.trafficLight.posY;
								}
								
								if (s1Elem->data.trafficLight.posZ.valid && s2Elem->data.trafficLight.posZ.valid)
								{
									newElem.data.trafficLight.posZ.valid = TRUE;
									newElem.data.trafficLight.posZ.value = s1Elem->data.trafficLight.posZ.value * w1 + 
										s2Elem->data.trafficLight.posZ.value * w2;
								}
								else if (s1Elem->data.trafficLight.posZ.valid)
								{
									newElem.data.trafficLight.posZ = s1Elem->data.trafficLight.posZ;
								}
								else if (s2Elem->data.trafficLight.posZ.valid)
								{
									newElem.data.trafficLight.posZ = s2Elem->data.trafficLight.posZ;
								}
								
								if (s1Elem->data.trafficLight.stopPosX.valid && s2Elem->data.trafficLight.stopPosX.valid)
								{
									newElem.data.trafficLight.stopPosX.valid = TRUE;
									newElem.data.trafficLight.stopPosX.value = s1Elem->data.trafficLight.stopPosX.value * w1 + 
										s2Elem->data.trafficLight.stopPosX.value * w2;
								}
								else if (s1Elem->data.trafficLight.stopPosX.valid)
								{
									newElem.data.trafficLight.stopPosX = s1Elem->data.trafficLight.stopPosX;
								}
								else if (s2Elem->data.trafficLight.stopPosX.valid)
								{
									newElem.data.trafficLight.stopPosX = s2Elem->data.trafficLight.stopPosX;
								}
								
								if (s1Elem->data.trafficLight.stopPosY.valid && s2Elem->data.trafficLight.stopPosY.valid)
								{
									newElem.data.trafficLight.stopPosY.valid = TRUE;
									newElem.data.trafficLight.stopPosY.value = s1Elem->data.trafficLight.stopPosY.value * w1 + 
										s2Elem->data.trafficLight.stopPosY.value * w2;
								}
								else if (s1Elem->data.trafficLight.stopPosY.valid)
								{
									newElem.data.trafficLight.stopPosY = s1Elem->data.trafficLight.stopPosY;
								}
								else if (s2Elem->data.trafficLight.stopPosY.valid)
								{
									newElem.data.trafficLight.stopPosY = s2Elem->data.trafficLight.stopPosY;
								}
								break;
								
							case TrafficElementType::StopLine:
								newElem.data.stopLine.distance = s1Elem->data.stopLine.distance * w1 + 
									s2Elem->data.stopLine.distance * w2;
									
								if (s1Elem->data.stopLine.heading.valid && s2Elem->data.stopLine.heading.valid)
								{
									newElem.data.stopLine.heading.valid = TRUE;
									newElem.data.stopLine.heading.value = s1Elem->data.stopLine.heading.value * w1 + 
										s2Elem->data.stopLine.heading.value * w2;
								}
								else if (s1Elem->data.stopLine.heading.valid)
								{
									newElem.data.stopLine.heading = s1Elem->data.stopLine.heading;
								}
								else if (s2Elem->data.stopLine.heading.valid)
								{
									newElem.data.stopLine.heading = s2Elem->data.stopLine.heading;
								}
								break;
								
							case TrafficElementType::SpeedBump:
								newElem.data.speedBump.distance = s1Elem->data.speedBump.distance * w1 + 
									s2Elem->data.speedBump.distance * w2;
									
								if (s1Elem->data.speedBump.heading.valid && s2Elem->data.speedBump.heading.valid)
								{
									newElem.data.speedBump.heading.valid = TRUE;
									newElem.data.speedBump.heading.value = s1Elem->data.speedBump.heading.value * w1 + 
										s2Elem->data.speedBump.heading.value * w2;
								}
								else if (s1Elem->data.speedBump.heading.valid)
								{
									newElem.data.speedBump.heading = s1Elem->data.speedBump.heading;
								}
								else if (s2Elem->data.speedBump.heading.valid)
								{
									newElem.data.speedBump.heading = s2Elem->data.speedBump.heading;
								}
								break;
								
							case TrafficElementType::ArrowMark:
								newElem.data.arrowMark.arrows = w1 > w2 ? 
									s1Elem->data.arrowMark.arrows : s2Elem->data.arrowMark.arrows;
									
								if (s1Elem->data.arrowMark.posX.valid && s2Elem->data.arrowMark.posX.valid)
								{
									newElem.data.arrowMark.posX.valid = TRUE;
									newElem.data.arrowMark.posX.value = s1Elem->data.arrowMark.posX.value * w1 + 
										s2Elem->data.arrowMark.posX.value * w2;
								}
								else if (s1Elem->data.arrowMark.posX.valid)
								{
									newElem.data.arrowMark.posX = s1Elem->data.arrowMark.posX;
								}
								else if (s2Elem->data.arrowMark.posX.valid)
								{
									newElem.data.arrowMark.posX = s2Elem->data.arrowMark.posX;
								}
								
								if (s1Elem->data.arrowMark.posY.valid && s2Elem->data.arrowMark.posY.valid)
								{
									newElem.data.arrowMark.posY.valid = TRUE;
									newElem.data.arrowMark.posY.value = s1Elem->data.arrowMark.posY.value * w1 + 
										s2Elem->data.arrowMark.posY.value * w2;
								}
								else if (s1Elem->data.arrowMark.posY.valid)
								{
									newElem.data.arrowMark.posY = s1Elem->data.arrowMark.posY;
								}
								else if (s2Elem->data.arrowMark.posY.valid)
								{
									newElem.data.arrowMark.posY = s2Elem->data.arrowMark.posY;
								}
								break;
								
							case TrafficElementType::ProhibitionZone:
								newElem.data.prohibitionZone.centerX = s1Elem->data.prohibitionZone.centerX * w1 + 
									s2Elem->data.prohibitionZone.centerX * w2;
								newElem.data.prohibitionZone.centerY = s1Elem->data.prohibitionZone.centerY * w1 + 
									s2Elem->data.prohibitionZone.centerY * w2;
								newElem.data.prohibitionZone.width = s1Elem->data.prohibitionZone.width * w1 + 
									s2Elem->data.prohibitionZone.width * w2;
								newElem.data.prohibitionZone.length = s1Elem->data.prohibitionZone.length * w1 + 
									s2Elem->data.prohibitionZone.length * w2;
									
								if (s1Elem->data.prohibitionZone.heading.valid && s2Elem->data.prohibitionZone.heading.valid)
								{
									newElem.data.prohibitionZone.heading.valid = TRUE;
									newElem.data.prohibitionZone.heading.value = s1Elem->data.prohibitionZone.heading.value * w1 + 
										s2Elem->data.prohibitionZone.heading.value * w2;
								}
								else if (s1Elem->data.prohibitionZone.heading.valid)
								{
									newElem.data.prohibitionZone.heading = s1Elem->data.prohibitionZone.heading;
								}
								else if (s2Elem->data.prohibitionZone.heading.valid)
								{
									newElem.data.prohibitionZone.heading = s2Elem->data.prohibitionZone.heading;
								}
								break;
								
							case TrafficElementType::ParkingSlot:
								newElem.data.parkingSlot.centerX = s1Elem->data.parkingSlot.centerX * w1 + 
									s2Elem->data.parkingSlot.centerX * w2;
								newElem.data.parkingSlot.centerY = s1Elem->data.parkingSlot.centerY * w1 + 
									s2Elem->data.parkingSlot.centerY * w2;
								newElem.data.parkingSlot.width = s1Elem->data.parkingSlot.width * w1 + 
									s2Elem->data.parkingSlot.width * w2;
								newElem.data.parkingSlot.length = s1Elem->data.parkingSlot.length * w1 + 
									s2Elem->data.parkingSlot.length * w2;
								newElem.data.parkingSlot.occupancy = w1 > w2 ? 
									s1Elem->data.parkingSlot.occupancy : s2Elem->data.parkingSlot.occupancy;
									
								if (s1Elem->data.parkingSlot.heading.valid && s2Elem->data.parkingSlot.heading.valid)
								{
									newElem.data.parkingSlot.heading.valid = TRUE;
									newElem.data.parkingSlot.heading.value = s1Elem->data.parkingSlot.heading.value * w1 + 
										s2Elem->data.parkingSlot.heading.value * w2;
								}
								else if (s1Elem->data.parkingSlot.heading.valid)
								{
									newElem.data.parkingSlot.heading = s1Elem->data.parkingSlot.heading;
								}
								else if (s2Elem->data.parkingSlot.heading.valid)
								{
									newElem.data.parkingSlot.heading = s2Elem->data.parkingSlot.heading;
								}
								break;
								
							case TrafficElementType::ZebraCrossing:
								newElem.data.zebraCrossing.nearDistance = s1Elem->data.zebraCrossing.nearDistance * w1 + 
									s2Elem->data.zebraCrossing.nearDistance * w2;
									
								if (s1Elem->data.zebraCrossing.farDistance.valid && s2Elem->data.zebraCrossing.farDistance.valid)
								{
									newElem.data.zebraCrossing.farDistance.valid = TRUE;
									newElem.data.zebraCrossing.farDistance.value = s1Elem->data.zebraCrossing.farDistance.value * w1 + 
										s2Elem->data.zebraCrossing.farDistance.value * w2;
								}
								else if (s1Elem->data.zebraCrossing.farDistance.valid)
								{
									newElem.data.zebraCrossing.farDistance = s1Elem->data.zebraCrossing.farDistance;
								}
								else if (s2Elem->data.zebraCrossing.farDistance.valid)
								{
									newElem.data.zebraCrossing.farDistance = s2Elem->data.zebraCrossing.farDistance;
								}
								
								if (s1Elem->data.zebraCrossing.heading.valid && s2Elem->data.zebraCrossing.heading.valid)
								{
									newElem.data.zebraCrossing.heading.valid = TRUE;
									newElem.data.zebraCrossing.heading.value = s1Elem->data.zebraCrossing.heading.value * w1 + 
										s2Elem->data.zebraCrossing.heading.value * w2;
								}
								else if (s1Elem->data.zebraCrossing.heading.valid)
								{
									newElem.data.zebraCrossing.heading = s1Elem->data.zebraCrossing.heading;
								}
								else if (s2Elem->data.zebraCrossing.heading.valid)
								{
									newElem.data.zebraCrossing.heading = s2Elem->data.zebraCrossing.heading;
								}
								break;
							
							default:
								validType = FALSE;	
								break;
							}
							
							if (validType) elementsX.append(newElem);
							break;
						}
					}
				}
				
				so.elements = elementsX.toArray();
				return output.as<SessionSample>();
			}

			static OptionalDouble convert(OptBool val)
			{
				return val.valid ? (val.value ? 1 : 0) : OptionalDouble();
			}
			static OptionalDouble convert(OptDouble val)
			{
				return val.valid ? val.value : OptionalDouble();
			}
		};

	public:
		SPADAS_CLASS("traffic_sample_v2.TrafficSample", TrafficSampleVars)

		TrafficSample() : SessionSample(new TrafficSampleVars)
		{}
		TrafficSampleData *operator ->()
		{
			return &var()->data;
		}

		static String getProtocolName(UInt channel)
		{
			return (String)"traffic-sample-v2@" + channel;
		}
		SessionGeneralSample toGeneralSample()
		{
			return var()->toGeneralSample();
		}

		static Array<String> getProtocolNames(UInt channel)
		{
			return Array<String>::create(2, "traffic-sample-v1", "traffic-sample-v2").convert<String>([channel](auto& s){ return s + "@" + channel; });
		}
		Bool fromGeneralSample(String protocol, SessionGeneralSample generalSample)
		{
			return fromSample(protocol, generalSample);
		}
	};
}

#endif
